#include "quant.h"

const D_CONSTQ B0(0.35355339059327376220);
const D_CONSTQ B1(0.25489778955207958447);
const D_CONSTQ B2(0.27059805007309849220);
const D_CONSTQ B3(0.30067244346752264027);
const D_CONSTQ B4(0.35355339059327376220);
const D_CONSTQ B5(0.44998811156820785231);
const D_CONSTQ B6(0.65328148243818826392);
const D_CONSTQ B7(1.28145772387075308943);

const D_CONSTQ first_stage[8] = { B0, B1, B2, B3, B4, B5, B6, B7, };

void fdct_fold_q(D_QUANT_TABLE* in, D_QUANT* out) {
  int i;
  D_QUANT v;
  D_QUANT_TABLE q;
  for (i = 0; i < 64; ++i) {
    v = first_stage[i >> 3];
    v *= first_stage[i & 7];
    q =  in[i];
    out[i] = v / q;
  }
}

D_QUANT_TABLE stdlqt[] = {
    16, 11, 10, 16,  24,  40,  51,  61,
    12, 12, 14, 19,  26,  58,  60,  55,
    14, 13, 16, 24,  40,  57,  69,  56,
    14, 17, 22, 29,  51,  87,  80,  62,
    18, 22, 37, 56,  68, 109, 103,  77,
    24, 35, 55, 64,  81, 104, 113,  92,
    49, 64, 78, 87, 103, 121, 120, 101,
    72, 92, 95, 98, 112, 100, 103,  99 };

D_QUANT_TABLE  stdcqt[] = {
    17, 18, 24, 47, 99, 99, 99, 99,
    18, 21, 26, 66, 99, 99, 99, 99,
    24, 26, 56, 99, 99, 99, 99, 99,
    47, 66, 99, 99, 99, 99, 99, 99,
    99, 99, 99, 99, 99, 99, 99, 99,
    99, 99, 99, 99, 99, 99, 99, 99,
    99, 99, 99, 99, 99, 99, 99, 99,
    99, 99, 99, 99, 99, 99, 99, 99 };

D_QUANT lqt_[64];
D_QUANT cqt_[64];

void quant_init() {
  fdct_fold_q(stdlqt, lqt_);
  fdct_fold_q(stdcqt, cqt_);
}

void quant_l_run(fifo<D_DCT> *IN, fifo<D_QUANT_OUT> *OUT) {
  int i;
  while (IN->len() >= 64) {
    for (i = 0; i<64; ++i) {
      OUT->write((D_QUANT_OUT) (IN->read() * lqt_[i] + 0.5));
    }
  }
}

void quant_c_run(fifo<D_DCT> *IN, fifo<D_QUANT_OUT> *OUT) {
  int i;
  while (IN->len() >= 64) {
    for (i = 0; i<64; ++i) {
      OUT->write((D_QUANT_OUT) (IN->read() * cqt_[i] + 0.5));
    }
  }
}

